#include "ViewerReader.h"
#include <boost/filesystem.hpp>
#include <algorithm>
#include <iostream>

namespace Viewer
{
template<typename T>
int FscanfOrDie(FILE *fptr, const char *format, T* value)
{
    return fscanf(fptr, format, value);
}
// tool function for substract sub string between word_left and word_right.
// for example: 
//          string input_string = "../../example.cpp";
//          string output_string = GetSubStringBetween(input_string, "/", ".cpp");
// Then we get:
//          output_string = "example"
std::string GetSubStringBetween(const std::string& input_string, const std::string& word_left, const std::string& word_right)
{
    std::size_t right_pos = input_string.rfind(word_right); 
    std::size_t left_pos = input_string.rfind(word_left) + word_left.length();
    return input_string.substr(left_pos,right_pos);                  
}

ViewerReader::ViewerReader(std::string filename)
{
    boost::filesystem::path folderpath(filename);
    if(!boost::filesystem::exists(folderpath)) {
        std::cerr << "path: " << folderpath.string() <<  " does not exist..." << std::endl;
        return;
    }

    mvFileNames_.clear();

    // First, find the folder of Image
    // begin iteration for all files in this folder... 
    boost::filesystem::directory_iterator end_iter;
    for(boost::filesystem::directory_iterator file_iter(folderpath); file_iter!= end_iter; ++file_iter) {
        // check the file extension...
        if(file_iter->path().extension() == ".txt")
            mvFileNames_.push_back(file_iter->path().string());
    }

    std::sort(mvFileNames_.begin(), mvFileNames_.end(), [&](const std::string file_a,const std::string file_b){
        auto ia = GetSubStringBetween(file_a, "/", ".txt");
        auto ib = GetSubStringBetween(file_b, "/", ".txt");
        return std::stoi(ia) < std::stoi(ib);
    });
}

void ViewerReader::readAllFiles()
{
    std::for_each(mvFileNames_.begin(), mvFileNames_.end(), [this](std::string fileName){
        this->readSingleFile(fileName);
        this->mvvPoseVariables_.push_back(this->mvPoseVariables_);
    });

    // auto pose = mvvPoseVariables_[6].back();
    // for(size_t i = 0; i < mvvPoseVariables_[6].size(); i++){
    //     auto pose = mvvPoseVariables_[6].at(i);
    //     std::copy(pose.begin(), pose.end(), std::ostream_iterator<double>(std::cout, " "));
    //     std::cout << std::endl;
    // }

    std::cout << "mvvPoseVariables[6]: " << mvvPoseVariables_[6].size() << std::endl;
}

void ViewerReader::readSingleFile(std::string filename)
{
    std::vector<std::array<double,12> > vPoseVariables;
    vPoseVariables.reserve(1000);
    
    // mFramePosesGroundTruth.reserve(2000);
    // mLandMarkGroundTruth.reserve(2000);
    auto posefilename = filename;
    FILE *fptr = fopen(posefilename.c_str(), "r");
    if(!fptr)
        std::cerr << "failed to open file : " << posefilename << std::endl;
    while (!feof(fptr))
    {
        // first read the pose for this frame.
        size_t id;
        FscanfOrDie(fptr, "%d", &id);
        PoseType /*std::array<double,12>*/ temPose;
        // std::cout << "reading poses.. " << std::endl;
        std::for_each(temPose.begin(), temPose.end(), [&](double &data) {
            FscanfOrDie(fptr, "%lf", &data);
        });

        vPoseVariables.push_back(temPose);
    }
    
    std::cout <<"name:"<< posefilename <<  vPoseVariables.size() << std::endl;
    mvPoseVariables_ = vPoseVariables;

    fclose(fptr);
}
}